﻿///
/// NetworkAlgorithmAStar.fs
/// Defines parsing elements.
///

namespace PathFinder.PathFindingAlgorithms

open System
open System.IO
open System.Collections
open System.Collections.Generic
open System.Text.RegularExpressions
open Microsoft.FSharp

open PathFinder.Network

#light

///
/// Contains the most important elements for finding minimal paths using A*.
///
module public NetworkAlgorithmAStar =
    ///
    ///
    ///
    type public Cost = double
    ///
    /// StarNode.
    ///
    type public StarNode = {
        Node: NetworkNode;
        FCost: Cost;
        GCost: Cost;
        HCost: Cost;
        Parent: NetworkNode option;
        }
    ///
    /// Prints a StarNode.
    ///
    let public PrintStarNode snode =
        let res = "StarNode: Node = " + (snode.Node.ToString ()) + " | F = " + (snode.FCost.ToString ()) + " | G = " + (snode.GCost.ToString ())+ " | H = " + (snode.HCost.ToString ()) + " | Par = "
        match snode.Parent with
        | Some (node) -> res + (node.ToString ())
        | None -> res + "None"
    ///
    /// Prints the final path.
    ///
    let rec public PrintPath (out: string) path =
        match path with
        | [] -> 
            let fin = [for i in 0 .. (out.Length - 1 - 4) -> out.[i]] |> List.toArray
            String (fin)
        | h :: t -> 
            PrintPath (out + h + " -> ") t
    ///
    /// Path reconstructor.
    ///
    let rec public RebuildPath finalpath nodemap node = 
        let init lst = 
            match lst with
            | [] -> [node]
            | _ -> lst
        try
            let par = Map.find node nodemap
            RebuildPath (par :: (init finalpath)) nodemap par
        with
            | :? KeyNotFoundException -> finalpath
    ///
    /// Update function.
    /// This function will not change CL, it changes OL managing with parents and adding stuff. The OL list will have the same elements at the end.
    /// <param name="ol">The list representing the open list. Type: list of StarNode.</param>
    /// <param name="cl">The list representing the closed list. Type: list of StarNode.</param>
    /// <param name="neigh">The list representing the neighbourhood. Type: list of StarNode.</param>
    /// <param name="wn">The Working Node. Type: StarNode</param>
    /// Elements in neigh have wn as parent.
    ///
    let rec public Update ol cl neigh wn come_from =
        (* Verifying out condition -> Is neigh empty? *)
        match neigh with
        | [] ->
            (* Recursion breakage *) 
            (ol, cl, come_from)
        | h :: neigh_tail ->
            (* WN is not necessary the parent of H. *)
            (* Browsing neighbours *)
            (* Testing whether h belongs to CL *)
            if List.exists (fun x -> x.Node.Name = h.Node.Name) cl then
                (* The head is in CL, we must not consider it *)
                Update ol cl neigh_tail wn come_from (* Call recursive *)
            else
                (* The head is not in CL *)
                let testG = wn.GCost + (wn.Node.GetWeightToNeighbour (h.Node.Name)) (* Calculating testG, cumulative GCost of WN and cost WN -> H *)
                (* Testing whether h belongs to OL *)
                if List.exists (fun x -> x.Node.Name = h.Node.Name) ol then
                    (* The head is in OL *)
                    (* Find the node that is in ol which is equal to h *)
                    let comp_node = List.find (fun x -> x.Node.Name = h.Node.Name) ol
                    (* If I go to target passing from this neighbour h, will this be a better path than the one if I were in the node in ol? *)
                    if testG < comp_node.GCost then
                        (* Having a better way *)
                        (* Changing the parent to WN *)
                        let h2 = {Node = h.Node; FCost = testG (*h.FCost*); GCost = testG (*h.GCost*); HCost = h.HCost; Parent = Some wn.Node} (* WN is now the parent of H *)
                        (* REPLACING, in OL, the H with the new parent *)
                        let replacer (x) =
                            if x.Node.Name = h.Node.Name then
                                h2
                            else
                                x
                        let ol2 = List.map (fun x -> (replacer x)) ol
                        (* Sorting list by F score *)
                        let ol3 = List.sortBy (fun x -> x.FCost) ol2
                        (* Updating come_from *)
                        let come_from2 = Map.add h.Node.Name wn.Node.Name come_from
                        Update ol3 cl neigh_tail wn come_from2 (* Call recursive *)
                    else
                        (* Not having a better way *)
                        (* Changing values *)
                        let h2 = {Node = h.Node; FCost = testG (*h.FCost*); GCost = testG (*h.GCost*); HCost = h.HCost; Parent = h.Parent}
                        (* REPLACING, in OL, the H inside with the new parent *)
                        let replacer (x) =
                            if x.Node.Name = h.Node.Name then
                                h2
                            else
                                x
                        let ol2 = List.map (fun x -> (replacer x)) ol
                        (* Sorting list by F score *)
                        let ol3 = List.sortBy (fun x -> x.FCost) ol2
                        Update ol3 cl neigh_tail wn come_from (* Call recursive *)
                else
                    (* The head is not in OL *)
                    (* Setting the parent to WN *)
                    let h2 = {Node = h.Node; FCost = testG (*h.FCost*); GCost = testG (*h.GCost*); HCost = h.HCost; Parent = Some wn.Node} (* WN is now the parent of H *)
                    (* ADDING to OL the H with the new parent *)
                    let ol2 = h2 :: ol
                    (* Sorting list by F score *)
                    let ol3 = List.sortBy (fun x -> x.FCost) ol2
                    (* Updating come_from *)
                    let come_from2 = Map.add h.Node.Name wn.Node.Name come_from
                    Update ol3 cl neigh_tail wn come_from2 (* Call recursive *)
    ///
    /// Scanner function.
    ///
    let rec public Scan ol cl goal come_from =
        (* Verifying out condition *)
        (* Is ol empty? *)
        match ol with
        | [] ->
            (* Recursion breakage *)
            (cl, come_from) (* Returning final value, we finished examining ol *)
        | h :: ol_tail ->
            (* Browsing OL, h is the node having the lowest fscore, ol is ordered *)
            (* Checking whether the head is the goal *)
            if h.Node.Name = goal.Node.Name then
                (* Path found! *)
                (cl, come_from) (* Returning path *)
            else
                (* We have to process more nodes *)
                (* Evaluating some quantities *)
                let ol2 = ol_tail (* Removing from OL the head *)
                let cl2 = h :: cl (* Inserting the head in CL *)
                (* Obtaining the neighbours of h and creating starnodes *)
                //let starnodes = List.map (fun (x: NetworkNodeNeighbour) -> {StarNode.Node = x.Node; StarNode.FCost = h.GCost + x.Weight; StarNode.GCost = h.GCost + x.Weight; StarNode.HCost = 0.0; StarNode.Parent = None}) h.Node.Neighbourhood (* Some h.Node in Parent *) (** THIS LINE WORKS NOT USING HCost **)
                let starnodes = List.map (fun (x: NetworkNodeNeighbour) -> {StarNode.Node = x.Node; StarNode.FCost = h.GCost + x.Weight + Norm (x.Node, goal.Node); StarNode.GCost = h.GCost + x.Weight; StarNode.HCost = Norm (x.Node, goal.Node); StarNode.Parent = None}) h.Node.Neighbourhood (* Some h.Node in Parent *) (** THIS LINE WORKS USING HCost **)
                (* Managing these lists *)
                let (ol3, cl3, come_from2) = Update ol2 cl2 starnodes h come_from
                (* Returning *)
                Scan ol3 cl3 goal come_from2 (* Call recursive *)
    ///
    /// Main algorithm.
    /// Nodes are Star nodes
    ///
    let public AStar start goal =
        let norm_start = Norm (start, goal)
        let start1 = {Node = start; FCost = norm_start; GCost = 0.0; HCost = norm_start; Parent = None}
        let goal1 = {Node = goal; FCost = 0.0; GCost = 0.0; HCost = 0.0; Parent = None}
        let ol = [start1]
        let cl = []
        Scan ol cl goal1 Map.empty
